#include "../driver/conn_uart.h"
#include "../conn/proto.h"
#include "../driver/num_led.h"
#include "../driver/motor.h"
#include <stdbool.h>
#include <string.h>
#include "../driver/debug_io.h"

#define RECV_TIMEOUT 100
#define RECV_BUF_SIZE 30
#define PROTO_EMPTY_SIZE 5
#define ARG_INDEX 3
static ByteView recvTemp;
static uint8_t recvBuf[RECV_BUF_SIZE];

static bool recv(uint32_t timeout) {
    return osOK == conn_uart_poll(&recvTemp, timeout);
}

static bool take(uint8_t *p_value, uint32_t timeout) {
    if (recvTemp.len <= 0) {
        bool ret = recv(timeout);
        if (!ret) {
            return ret;
        }
    }
    *p_value = recvTemp.buf[0];
    recvTemp.buf += 1;
    recvTemp.len -= 1;
    return true;
}

static bool take_buf(uint8_t *dst, uint16_t len, uint32_t timeout) {
    uint16_t index = 0;
    for (; index < len; ) {
        if (recvTemp.len <= 0) {
            bool ret = recv(timeout);
            if (!ret) {
                return ret;
            }
        }
        uint16_t n = len - index;
        if (n > recvTemp.len) {
            n = recvTemp.len;
        }
        memcpy(dst + index, recvTemp.buf, n);
        index += n;
        recvTemp.buf += index;
        recvTemp.len -= index;
    }
    return true;
}

static bool sync() {
    uint8_t value;
    bool flag = false;
    for (;;) {
        if (!take(&value, RECV_TIMEOUT)) {
            return false;
        }
        if (flag && (value == HEAD1)) {
            break;
        }
        flag = value == HEAD0;
    }
    return true;
}

static void exec(const uint8_t *buf, uint8_t len);

static void recv_all() {
    uint8_t len;
    if (!take(&len, RECV_TIMEOUT)) {
        return;
    }
    if ((len < 7) || (len > RECV_BUF_SIZE)) {
        return;
    }
    len = len - 3;
    
    if (!take_buf(recvBuf, len, RECV_TIMEOUT)) {
        return;
    }

    uint8_t sum = proto_xor_sum(recvBuf, len - 2);
    if (sum != recvBuf[len - 2]) {
        return;
    }

    if (END != recvBuf[len - 1]) {
        return;
    }

    exec(recvBuf, len);
}

static uint8_t exec_motor_ctrl(const uint8_t *buf, uint8_t len) {
    if (len < (PROTO_EMPTY_SIZE + 2)) {
        return EC_PARSE;
    }
    uint8_t id = buf[ARG_INDEX];
    uint8_t dir = buf[ARG_INDEX + 1];
    return motor_debug_ctrl(id, dir);
}

static uint8_t exec_num_led_ctrl(const uint8_t *buf, uint8_t len) {
    if (len < (PROTO_EMPTY_SIZE + 3)) {
        return EC_PARSE;
    }
    uint8_t id = buf[ARG_INDEX];
    uint16_t num = (buf[ARG_INDEX + 1] << 8) + buf[ARG_INDEX + 2];
    if (id == 1) {
        num_led_top_show(num);
    } else {
        num_led_bottom_show(num);
    }
    return EC_OK;
}

static uint8_t exec_debug_io(const uint8_t *buf, uint8_t len) {
    if (len < (PROTO_EMPTY_SIZE + 3)) {
        return EC_PARSE;
    }
    uint8_t port = buf[ARG_INDEX];
    uint8_t pin = buf[ARG_INDEX + 1];
    uint8_t level = buf[ARG_INDEX + 2];
    debug_io_ctrl(port, pin, level);
    return EC_OK;
}

static void exec(const uint8_t *buf, uint8_t len) {
    uint8_t type = buf[0];
    uint8_t seq = buf[1];
    uint8_t cmd = buf[2];
    uint8_t ec = EC_INVALID;
    switch (cmd) {
        case CMD_MOTOR_CTRL: ec = exec_motor_ctrl(buf, len); break;
        case CMD_NUM_LED_CTRL: ec = exec_num_led_ctrl(buf, len); break;
        case CMD_DEBUG_IO: ec = exec_debug_io(buf, len); break;
    }
    conn_write_simple_res(seq, cmd, ec);
}

void cmd_recv_loop() {

    for (;;) {
        recv(osWaitForever);
        if (!sync()) {
            continue;
        }
        recv_all();
    }
}
